import wx
from Gui import *  #Import the Gui
from CRC import * #import the Checksum functions
from RR_fiducials import *
import serial
import time

def bindControls():
    frame_1.Comm_Connect.Bind(wx.EVT_BUTTON, serialConnect)
    frame_1.Forward_Val.Bind(wx.EVT_SCROLL,onScroll)
    frame_1.Turn_Val.Bind(wx.EVT_SCROLL,onScroll)
    
def onScroll(event):
    # position is an int value
    forward = frame_1.Forward_Val.GetValue()
    Turn = frame_1.Turn_Val.GetValue()
    sendCommand(forward,Turn)
    
def sendCommand(Forward,Turn):
    
    if Forward < 0:
        Forward = 0xFFFF + Forward + 1
    if Turn < 0:
        Turn = 0xFFFF + Turn + 1

#Make up the command string starting with M for the motion command
#Then the Event ID i.e 0 for stridge length 1 for stride height 2 for swing etc
#Then the value to be send and finally a set of trailing zeros
    sendstring = "H"+"%0.4X"%Forward+"%0.4X"%Turn+"0000"

    #Calculate Checksum
    Checksum=CRC_CCITT()
    crcval=0x1D0F  #initialize for CCITT 0x1DOF
    for c in sendstring:
        crcval=Checksum.update_crc(crcval, ord(c))
    sendstring = "[" + sendstring + "%0.4X"%crcval + "]"
    print (sendstring+" ")
    try:
        ser.write(sendstring+chr(13))
        time.sleep(0.01)
    except:
        print (" Serial Port Failure ")

def serialConnect(event):
    global ser
    try:
        ser = serial.Serial((int(frame_1.Comm_Port.Value)-1),57600)
        print (" Connected to "+str(ser.portstr))
        
    except:
        print (" Invalid Comm Port ")


#Joystick Timer
def onTimer(*args, **kw):
    speed,orient = Bioloid_Visual_Navigation_Control()
    print speed,orient
#    ##sendCommand(4,0)
    sendCommand(speed,orient)
    ObjectTimer.Restart() # make the timer get called again:
########################
# Start the wx application handler
########################
if __name__ == "__main__":
    app = wx.PySimpleApp(0)
    wx.InitAllImageHandlers()
    frame_1 = MyFrame(None, -1, "")
    bindControls()
    Initialize_Vision_API()
    
    ObjectTimer = wx.CallLater(100, onTimer)
    app.SetTopWindow(frame_1)
    frame_1.Show()
    app.MainLoop()


    

